#include "pid_controller.h"


pid_ctrl_f32_t pid1 = 
{
  .err = 0.0f,
  .ref = 750.0f,
  .in = 560.0f,
  .Up = 0.0f,
  .Ui = 0.0f,
  .Ud = 0.0f,
  .Ui_max = 20.0f,
  .Ui_min = -20.0f,
  .Kp = 0.8f,
  .Ki = 0.2f,
  .Kd = 0.0f,
  .out = 0.0f,
};

int32_t pid_controller_init_f32(pid_ctrl_f32_t *pid_ctrl)
{
  return 0;
}


int32_t pid_controller_update_f32(pid_ctrl_f32_t *pid_ctrl)
{
  pid_ctrl->err = pid_ctrl->ref - pid_ctrl->in;
  
  /* 比例项 */
  pid_ctrl->Up = pid_ctrl->err * pid_ctrl->Kp;
  
  /* 积分项 */
  pid_ctrl->Ui = (pid_ctrl->Ui + pid_ctrl->err) * pid_ctrl->Ki;
  if (pid_ctrl->Ui > pid_ctrl->Ui_max)
  {
    pid_ctrl->Ui = pid_ctrl->Ui_max;
  }
  else if (pid_ctrl->Ui < pid_ctrl->Ui_min)
  {
    pid_ctrl->Ui = pid_ctrl->Ui_min;
  }
  
  /* 微分项 */
  pid_ctrl->Ud = pid_ctrl->Kd;
  
  pid_ctrl->out = pid_ctrl->Up + pid_ctrl->Ui + pid_ctrl->Ud;
  
  return 0;
}




#if (ARM_CMSIS_DSP_ENABLE == 1)

#include "arm_math.h" 

int32_t pid_init_f32(pid_instance_f32_t *pid, float Kp, float Ki, float Kd, 
  float max_output, float max_integral) 
{
  pid->Kp = Kp;
  pid->Ki = Ki;
  pid->Kd = Kd;
  pid->integral = 0.0f;
  pid->prev_error = 0.0f;
  pid->max_output = max_output;
  pid->max_integral = max_integral;
  
  return 0;
}


float pid_update_f32(pid_instance_f32_t *pid, float setpoint, float measurement)
{
  float error = setpoint - measurement;
  float derivative, temp_f[4], output;

  // 积分项（带抗饱和）
  pid->integral += error;
  arm_clip_f32(&pid->integral, &pid->integral, -pid->max_integral, pid->max_integral, 1);

  // 微分项（使用 CMSIS-DSP 的减法）
  arm_sub_f32(&error, &pid->prev_error, &derivative, 1);

  // 计算输出
  arm_mult_f32(&pid->Kp, &error, &temp_f[0], 1);
  arm_mult_f32(&pid->Ki, &pid->integral, &temp_f[1], 1);
  arm_mult_f32(&pid->Kd, &derivative, &temp_f[2], 1);
  output = temp_f[0] + temp_f[1] + temp_f[2]; 

  // 输出限幅
  arm_clip_f32(&output, &output, -pid->max_output, pid->max_output, 1);

  pid->prev_error = error;
  return output;
}

#endif



